#include "zf_common_headfile.h"

Balance_Param_TypeDef Balance_Param;

float Vetical_PWM = 0, Velocity_PWM = 0;
int Momentum_Motor_Duty = 0;

void Balance_Param_Init(void)
{
    Balance_Param.Angle_PID.kp = 10;
    Balance_Param.Angle_PID.ki = -0.02;
    Balance_Param.Angle_PID.kd = 0.1;

    Balance_Param.Velocity_PID.kp = 9;
    Balance_Param.Velocity_PID.ki = -0.01;
    Balance_Param.Velocity_PID.kd = 0;
}

float Vertical(float target_angle, float current_angle, float gyro_x)
{
    float Vetical_PWM, Bias = current_angle - target_angle;
    static float error;
    error += Bias;
    if (error > +300)
        error = +300;
    if (error < -300)
        error = -300;

    Vetical_PWM = Balance_Param.Angle_PID.kp * Bias +
                  Balance_Param.Angle_PID.ki * error +
                  Balance_Param.Angle_PID.kd * gyro_x;

    return Vetical_PWM;
}
float Velocity(int encoder)
{
    static float Encoder, Encoder_Integral;
    float Velocity_PWM, Encoder_Least;
    Encoder_Least = encoder;
    Encoder *= 0.7;
    Encoder += Encoder_Least * 0.3;
    Encoder_Integral += Encoder;

    if (Encoder_Integral > +500)
        Encoder_Integral = +500;
    if (Encoder_Integral < -500)
        Encoder_Integral = -500;

    Velocity_PWM = Balance_Param.Velocity_PID.kp * encoder +
                   Balance_Param.Velocity_PID.ki * Encoder_Integral / 100;
    return Velocity_PWM;
}

void Balance_Control(void)
{
    Vetical_PWM = Vertical(0, Pitch, Filter_GyroY);
    Velocity_PWM = Velocity(EncoderValue[1]);
    Momentum_Motor_Duty = (int)(Vetical_PWM + Velocity_PWM);
    FlyWheel_Control(Momentum_Motor_Duty);
}
